/*============================= Stand Up ==============================*/
/**
 * Transitionary state that is called for the robot to stand up into
 * balance control mode.
 */

#include "FSM_State_StandUp.h"

/**
 * Constructor for the FSM State that passes in state specific info to
 * the generic FSM State constructor.
 *
 * @param _controlFSMData holds all of the relevant control data
 */
template <typename T>
FSM_State_StandUp<T>::FSM_State_StandUp(ControlFSMData<T>* _controlFSMData)
    : FSM_State<T>(_controlFSMData, FSM_StateName::STAND_UP, "STAND_UP") {
    // Do nothing
    // Set the pre controls safety checks
    this->checkSafeOrientation = false;

    // Post control safety checks
    this->checkPDesFoot = false;
    this->checkForceFeedForward = false;
}

template <typename T>
void FSM_State_StandUp<T>::onEnter() {
    // Default is to not transition
    this->nextStateName = this->stateName;

    // Reset the transition data
    this->transitionData.zero();

    // Reset iteration counter
    iter = 0;
    _ini_foot_pos << 0., 0., -0.285;
}

/**
 * Calls the functions to be executed on each control loop iteration.
 */
template <typename T>
void FSM_State_StandUp<T>::run() {
    T hMax = -0.31;
    T hMin= -0.17;
    T iter_now = fmax(0, iter-1000);
    T progress = 0.2 * iter_now * this->_data->controlParameters->controller_dt;

    // if (progress > 1.) {
    //     progress = 1.;
    // }
    // for (int i = 0; i < 4; i++) {
    //     this->_data->_legController->commands[i].pDes = _ini_foot_pos;
    //     this->_data->_legController->commands[i].pDes[2] = progress * hMax + (1. - progress) * _ini_foot_pos(2);
    // }

    if (progress > 2.) {
        progress = 2.;
    }
    if (progress < 1) {
        for (int i = 0; i < 4; i++) {
            this->_data->_legController->commands[i].pDes = _ini_foot_pos;
            this->_data->_legController->commands[i].pDes[2] = progress * hMin + (1. - progress) * _ini_foot_pos(2);
        }
    } else {
        for (int i = 0; i < 4; i++) {
            this->_data->_legController->commands[i].pDes = _ini_foot_pos;
            this->_data->_legController->commands[i].pDes[2] =
                 (progress - 1) * hMax+(2. - progress) * hMin ;
        }
    }

    // for (int i = 0; i < 4; i++) {
    //     _ini_foot_pos[2] = hMax;
    //     this->_data->_legController->commands[i].pDes = _ini_foot_pos;
    // }

    Vec3<float> tempAng;
    for (int foot = 0; foot < 4; foot++) {
        computeAuxiliaryAngle(*this->_data->_quadruped, this->_data->_legController->commands[foot].pDes, &tempAng);
        this->_data->_legController->commands[foot].qDes[0] = tempAng(0);
        this->_data->_legController->commands[foot].qDes[1] = tempAng(1) + tempAng(2);
        this->_data->_legController->commands[foot].qDes[2] = tempAng(2) - tempAng(1);
            this->_data->_legController->commands[foot].qdDes<<0.0,0.0,0.0;
            this->_data->_legController->commands[foot].kpJoint << 120, 0, 0, 0, 85, 0, 0, 0, 85;
            this->_data->_legController->commands[foot].kdJoint << 1.5, 0, 0, 0, 1.8, 0, 0, 0, 1.8;
                            this->_data->_legController->commands[foot].tauFeedForward << 0, 0, 0;
                this->_data->_legController->commands[foot].forceFeedForward << 0, 0, 0;
    }
    this->_data->_legController->setControlMode(All_Pos);
    this->_data->_legController->setEnabled(true);
}

/**
 * Manages which states can be transitioned into either by the user
 * commands or state event triggers.
 *
 * @return the enumerated FSM state name to transition into
 */
template <typename T>
FSM_StateName FSM_State_StandUp<T>::checkTransition() {
    this->nextStateName = this->stateName;
    iter++;

    // Switch FSM control mode
    switch ((int)this->_data->controlParameters->control_mode) {
        case K_STAND_UP:
            break;
        case K_BALANCE_STAND:
            this->nextStateName = FSM_StateName::BALANCE_STAND;
            break;

        case K_LOCOMOTION:
            this->nextStateName = FSM_StateName::LOCOMOTION;
            break;

        case K_VISION:
            this->nextStateName = FSM_StateName::VISION;
            break;

        case K_PASSIVE:    // normal c
            this->nextStateName = FSM_StateName::PASSIVE;
            break;

        default:
            std::cout << "[CONTROL FSM] Bad Request: Cannot transition from " << K_PASSIVE << " to "
                      << this->_data->controlParameters->control_mode << std::endl;
    }

    // Get the next state
    return this->nextStateName;
}

/**
 * Handles the actual transition for the robot between states.
 * Returns true when the transition is completed.
 *
 * @return true if transition is complete
 */
template <typename T>
TransitionData<T> FSM_State_StandUp<T>::transition() {
    // Finish Transition
    switch (this->nextStateName) {
        case FSM_StateName::PASSIVE:    // normal
            this->transitionData.done = true;
            break;

        case FSM_StateName::BALANCE_STAND:
            this->transitionData.done = true;
            break;

        case FSM_StateName::LOCOMOTION:
            this->transitionData.done = true;
            break;

        case FSM_StateName::VISION:
            this->transitionData.done = true;
            break;

        default:
            std::cout << "[CONTROL FSM] Something went wrong in transition" << std::endl;
    }

    // Return the transition data to the FSM
    return this->transitionData;
}

/**
 * Cleans up the state information on exiting the state.
 */
template <typename T>
void FSM_State_StandUp<T>::onExit() {
    // Nothing to clean up when exiting
}

// template class FSM_State_StandUp<double>;
template class FSM_State_StandUp<float>;
